#include <stdio.h>

typedef struct {
	int resolution, size_x, size_y;
	float offset_x, offset_y;
	int min_x, max_x, min_y, max_y;
	float **prob;
} map_type;

void new_hornetsoft_map(map_type *map, int size_x, int size_y)
{
	int i;
	
	map->prob = (float **)calloc(size_x, sizeof(float *));
	for(i = 0; i < size_x; i++)
		map->prob[i] = (float *)calloc(size_y, sizeof(float));
}

int read_beesoft_map(char *mapName, map_type *map)
{
	int x, y, count;
	float temp;
	char line[256];
	FILE *fp;
	
	if((fp = fopen(mapName, "rt")) == NULL) {
		fprintf(stderr, "# Could not open file %s\n", mapName);
		return -1;
	}
	fprintf(stderr, "# Reading map: %s\n", mapName);
	while((fgets(line, 256, fp) != NULL)
		  && (strncmp("global_map[0]", line , 13) != 0)) {
		if(strncmp(line, "robot_specifications->resolution", 32) == 0)
			if(sscanf(&line[32], "%d", &(map->resolution)) != 0)
//				printf("# Map resolution: %d cm\n", map->resolution);
		if(strncmp(line, "robot_specifications->autoshifted_x", 35) == 0)
			if(sscanf(&line[35], "%g", &(map->offset_x)) != 0) {
				map->offset_x = map->offset_x;
//				printf("# Map offsetX: %g cm\n", map->offset_x);
			}
		if(strncmp(line, "robot_specifications->autoshifted_y", 35) == 0) {
			if (sscanf(&line[35], "%g", &(map->offset_y)) != 0) {
				map->offset_y = map->offset_y;
//				printf("# Map offsetY: %g cm\n", map->offset_y);
			}
		}
	}
	
	if(sscanf(line,"global_map[0]: %d %d", &map->size_y, &map->size_x) != 2) {
		fprintf(stderr, "ERROR: corrupted file %s\n", mapName);
		fclose(fp);
		return -1;
	}
//	printf("# Map size: %d %d\n", map->size_x, map->size_y);
	
	new_hornetsoft_map(map, map->size_x, map->size_y);
	
	map->min_x = map->size_x;
	map->max_x = 0;
	map->min_y = map->size_y;
	map->max_y = 0;
	count = 0;
	for(x = 0; x < map->size_x; x++)
		for(y = 0; y < map->size_y; y++, count++) {
//			if(count % 10000 == 0)
//				fprintf(stderr, "\r# Reading ... (%.2f%%)",
//						count / (float)(map->size_x * map->size_y) * 100);
			
			fscanf(fp,"%e", &temp);
			if(temp < 0.0)
				map->prob[x][y] = -1;
			else {
				if(x < map->min_x)
					map->min_x = x;
				else if(x > map->max_x)
					map->max_x = x;
				if(y < map->min_y)
					map->min_y = y;
				else if(y > map->max_y)
					map->max_y = y;
				map->prob[x][y] = 1 - temp;	   
			}
			printf("%e ",map->prob[x][y]);
		}
		printf("\n");
//	fprintf(stderr, "\r# Reading ... (%.2f%%)\n\n",
//			count / (float)(map->size_x * map->size_y) * 100);
	fclose(fp);
	return 0;
}

int main(int argc, char **argv)
{
	map_type map;
	
	read_beesoft_map(argv[1], &map);
	return 0;
}
